namespace NBody.KD
{
    using System;
    using NBody;
    
    public class PriorityQueue
    {
        private struct queue_member
        {
            public int queue;
            public double priority;
        }
        
        private int n;
        private int max_size;
        queue_member[] pq;
        
        public PriorityQueue(int max)
        {
            n = 0;
            max_size = max;
            pq = new queue_member[max + 1];
            for (int i = 0; i < pq.Length; i++)
            {
                pq[i].queue = -1;
                pq[i].priority = Double.PositiveInfinity;
            }
        }
        
        public bool IsEmpty
        {
            get
            {
                if (n == 0)
                    return true;
                else
                    return false;
            }
        }
        
        public int Size
        {
            get { return n; }
        }
        
        public void Reset()
        {
            n = 0;
        }
        
        public void Push(int p, double dist)
        {
            if (++n > max_size)
                throw new ApplicationException("Priority queue is full");
                
            int r = n;
            while (r > 1)
            {
                int pp = r>>1;
                if (pq[pp].priority >= dist)
                    break;
                pq[r] = pq[pp];
                r = pp;
            }
            pq[r].priority = dist;
            pq[r].queue = p;
        }
    
        public void Pop()
        {
            double kn = pq[n--].priority;
            int p = 1;
            int r = p<<1;
            while (r <= n)
            {
                if (r < n && pq[r].priority < pq[r+1].priority)
                    r++;
                if (kn >= pq[r].priority)
                    break;
                pq[p] = pq[r];
                p = r;
                r = p<<1;
            }
            pq[p] = pq[n+1];
        }
    
        public double TopPriority
        {
            get { return pq[1].priority; }
        }
        
        public int TopQueue
        {
            get { return pq[1].queue; }
        }
        
        /*public static void Main()
        {
            Random ran = new Random();
            PriorityQueue pq = new PriorityQueue(10);
            
            for (int i = 0; i < 10; i++)
            {
                pq.Push(ran.Next(), ran.NextDouble());
            }
            
            for (int i = 0; i < 10; i++)
            {
                Console.WriteLine("i = {0}, top dist = {1}, int = {2}", i, pq.TopPriority, pq.TopQueue);
                pq.Pop();
            }
        }*/
    }
}
            
